// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GroundTruthEstimator.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Navigation.Benchmark
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Navigation.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;

    /// <summary>
    /// Class for finding ground truth from an image
    /// </summary>
    public class GroundTruthEstimator : IDisposable
    {
        /// <summary>
        /// Track whether Dispose has been called.
        /// </summary>
        private bool disposed;

        /// <summary>
        /// The camera pose estimator
        /// </summary>
        private UpwardFacingCameraPoseEstimator cameraPoseEstimator;

        /// <summary>
        /// The robot pose estimator
        /// </summary>
        private RobotPoseEstimator robotPoseEstimator;

        /// <summary>
        /// Initializes a new instance of the <see cref="GroundTruthEstimator" /> class.
        /// </summary>
        /// <param name="configFolder">Path to a folder containing the calibration files</param>
        public GroundTruthEstimator(string configFolder)
        {
            this.cameraPoseEstimator = new UpwardFacingCameraPoseEstimator(configFolder, configFolder);
            this.robotPoseEstimator = new RobotPoseEstimator(configFolder);
        }

        /// <summary>
        /// Gets a value indicating whether the necessary calibration info is present.
        /// </summary>
        public bool HasCalibrationInfo
        {
            get
            {
                return (this.cameraPoseEstimator.HasCalibrationInfo && this.robotPoseEstimator.HasCalibrationInfo);
            }
        }

        /// <summary>
        /// Gets the number of landmarks in the camera pose estimators landmark info
        /// </summary>
        public int LandmarkCount
        {
            get
            {
                return this.cameraPoseEstimator.LandmarkCount;
            }
        }

        /// <summary>
        /// Attempt to get a pose from an image
        /// </summary>
        /// <param name="image">The YUY2 image</param>
        /// <param name="landmarkId">The landmark to use for evaluation</param>
        /// <param name="robotPose">The output pose (distances in millimeters, heading in radians)</param>
        /// <param name="cameraTranslations">The cameraTranslation output by the cameraPoseEstimator</param>
        /// <returns>True if successful</returns>
        public bool TryGetGroundTruthFromImage(ImageFrameYuy2 image, int landmarkId, out Pose2D robotPose, out Vector3 cameraTranslations)
        {
            robotPose = new Pose2D(double.NaN, double.NaN, double.NaN);
            cameraTranslations = new Vector3(double.NaN, double.NaN, double.NaN);

            if (!this.HasCalibrationInfo)
            {
                return false;
            }

            ImageFrameGray8 lumImage = ImageOperations2DInterop.ConvertYUY2ToGray8Image(image);
            Pose2D cameraPose;
            Vector3 rotations;
            Vector3 translations;
            if (this.cameraPoseEstimator.TryGetCameraPose(lumImage, out cameraPose, out rotations, out translations))
            {
                Landmark landmark = this.cameraPoseEstimator.LookupLandmark(landmarkId);
                if (landmark == null)
                {
                    return false;
                }

                Pose2D? pose = this.robotPoseEstimator.EstimateRobotPose(cameraPose, landmark.Pose, landmark.TiltCalibrationParams, translations.Z);
                if (pose != null)
                {
                    robotPose = new Pose2D(pose.Value.X * MathConstants.MilliunitsPerUnit, pose.Value.Y * MathConstants.MilliunitsPerUnit, pose.Value.Heading);
                    cameraTranslations = translations;
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else
            {
                return false;
            }
        }

        /// <summary>
        /// Implement IDisposable
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
            GC.SuppressFinalize(this);
        }

        /// <summary>
        /// Dispose resources.
        /// </summary>
        /// <param name="disposing">True if dispose is called by user code, false otherwise</param>
        protected virtual void Dispose(bool disposing)
        {
            // Check to see if Dispose has already been called. 
            if (!this.disposed)
            {
                if (disposing)
                {
                    if (this.cameraPoseEstimator != null)
                    {
                        this.cameraPoseEstimator.Dispose();
                        this.cameraPoseEstimator = null;
                    }
                }

                this.disposed = true;
            }
        }
    }
}
